#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
import math

cmddx = 0
cmddy = 0
cmddz = 0

def move_handler(data):
    global cmddx, cmddy, cmddz
    cmddx = data.linear.x
    cmddy = data.linear.y
    cmddz = data.angular.z

# def OdemetryToTF(odom):
#     tf_ = tf.Transformer()
#     tf_.

if __name__ == "__main__":
    rospy.init_node("visual_pose_node", anonymous=True)
    rospy.Subscriber("/robot0/cmd_vel", Twist, move_handler)

    loop_rate = 10.0
    rate = rospy.Rate(loop_rate)
    pos_x = 2.0
    pos_y = 2.0
    pos_z = 0.0
    pos_dx = 0.0
    pos_dy = 0.0
    pos_dz = 0.0

    local_dx = 0.0
    local_dy = 0.0
    local_dz = 0.0

    max_acc_x = 0.5
    max_acc_z = 0.5

    robot_odom = Odometry()
    robot_odom.header.frame_id = "map"
    pos_pub = rospy.Publisher("/robot0/odom", Odometry, queue_size=10)

    br = tf.TransformBroadcaster()

    while not rospy.is_shutdown():
        # update speed
        dx_diff = cmddx - local_dx
        if(dx_diff > 0):
            local_dx += min(max_acc_x/loop_rate, dx_diff)
        if(dx_diff < 0):
            local_dx -= min(max_acc_x/loop_rate, -dx_diff)

        dz_diff = cmddz - local_dz
        if (dz_diff > 0):
            local_dz += min(max_acc_z, dz_diff)
        if (dz_diff < 0):
            local_dz -= min(max_acc_z, -dz_diff)


        pos_dx = local_dx * math.cos(pos_z) - local_dy * math.sin(pos_z)
        pos_dy = local_dy * math.cos(pos_z) + local_dx * math.sin(pos_z)
        pos_dz = local_dz

        pos_x += pos_dx/loop_rate
        pos_y += pos_dy/loop_rate
        pos_z += pos_dz/loop_rate

        robot_odom.pose.pose.position.x = pos_x
        robot_odom.pose.pose.position.y = pos_y
        odom_quat = tf.transformations.quaternion_from_euler(0, 0,pos_z)
        robot_odom.pose.pose.orientation.x = odom_quat[0]
        robot_odom.pose.pose.orientation.y = odom_quat[1]
        robot_odom.pose.pose.orientation.z = odom_quat[2]
        robot_odom.pose.pose.orientation.w = odom_quat[3]
        robot_odom.header.stamp = rospy.Time.now()

        print (odom_quat)

        robot_odom.twist.twist.linear.x = cmddx
        robot_odom.twist.twist.linear.y = cmddy
        robot_odom.twist.twist.angular.z = cmddz

        pos_pub.publish(robot_odom)
        br.sendTransform((pos_x, pos_y, 0),
                         tf.transformations.quaternion_from_euler(0, 0,pos_z),
                         rospy.Time.now(),
                         "/robot0",
                         "/map"
                         )
        print("posx = %.2f, posy = %.2f, posz = %.2f"%(pos_x, pos_y, pos_z))
        rate.sleep()